#include "WPILib.h"

/**
	This is the Murray Calloway Simple Robot program for the 2010/2011 school
	year.  This code is designed to be sequential in nature to cut down on
	the complexity.  The main class MurrayCallowayRobot2011 is an instance
	of SimpleRobot, and has a constructor method, an autonomous method, and
	an operatorControl method.  The constructor method of course will be called
	upon initialization of the program.  The Autonomous and OperatorControl
	methods are currently envisioned to be called via input from either 1 or 2
	joystick buttons to switch modes.
		
	NOTES:
	Default position = travel position - not picking up from here
	Low position = lowest release postion AND also used for pickup
	Middle position = middle release
	Top position = top release
	Added a new variable = Solenoid
	
	TODO:
	DONE1. Study prebuilt autonomous line tracker
	DONE2. Implement line tracking into our code - don't forget to deploy our tube
	DONE3. Fully implement our solenoid for gripper arm (research how many solenoid
	4. Test autonomous as is to make sure that the inversion works
	DONE5. Implement joystick button variables (possibly)
	DONE6. Research gyro and any impact it may have
	DONE7. Implement possibility of joystick control arm
	DONE8. Research and implement encoder for secondary arm ()
	DONE9. slot numbers for digital inputs
	DONE10. Add init sequence for robot arm (make sure it is in default position)
	DONE11. semicolons!!!!!!
	DONE12. adjust variables for the arm positions
	DONE13. add get tube script
	14. fix encoder definition
	15. use joystick to determine the arm locations
	15. use the joystick to determine the encoder locations
	
*/

class MurrayCallowayRobot2011 : public SimpleRobot
{
	RobotDrive *myRobot;
	RobotDrive *myArm;
	Jaguar *mainArmController;
	Jaguar *secondaryArmController;
	Joystick *leftJoy;
	Joystick *rightJoy;
	// driver station object for getting selections
	DriverStation *ds;
	// digital inputs for line tracking sensors	
	DigitalInput *leftLineFollow;			
	DigitalInput *middleLineFollow;
	DigitalInput *rightLineFollow;
	// digital inputs for limit switches
	DigitalInput *limitMainTravelPosition;
	DigitalInput *limitSecondaryTravelPosition;
	// solenoids for gripper arm - pneumatics
	Solenoid *gripOpenSolenoid;
	Solenoid *gripCloseSolenoid;
	// encoders
	Encoder *encoderMainArm;
	Encoder *encoderSecondaryArm;
	
	//arm angle variables
	int Main;
	int Secondary;
	int previousAngle;
	int desiredAngle;
	bool armMovementDirection;
	
	//joystick variables
	int buttonNum;
	bool joyOutput;
	int lastButton;
	

	//arm varibles
	bool isDefaultArmPosition;
	//gripper
	bool gripState;
	
	//button variables
	static const UINT8 trigger = 1;	
	static const UINT8 low = 4;	
	static const UINT8 middle = 3;	
	static const UINT8 high = 5;	
	static const UINT8 load = 2;	
	static const UINT8 defaultArm = 7;	
	static const UINT8 grip = 6;	

public:
	MurrayCallowayRobot2011(void)
	{
		myRobot = new RobotDrive(1, 2);
		myArm = new RobotDrive(7, 8);
		//camera->GetInstance();
		mainArmController = new Jaguar(6,1);			///AAAAAHHH!!!!!!!!!
		secondaryArmController = new Jaguar(6,2);
		leftJoy = new Joystick(1);
		rightJoy = new Joystick(2);
		gripState = false;
		// digitial inputs for line following sensors.  Channel number may need
		// to be changed depending on physical robot setup
		leftLineFollow = new DigitalInput(4, 1);
		middleLineFollow = new DigitalInput(4, 2);
		rightLineFollow = new DigitalInput(4, 3);
		// digital inputs for limit switches for controlling where the main arm
		// rotates to stop
		limitMainTravelPosition = new DigitalInput(6, 1);
		limitSecondaryTravelPosition = new DigitalInput(6, 2);
		// driver station instance for digital I/O
		ds = DriverStation::GetInstance();
		//invert each of the motor outputs
		myRobot->SetInvertedMotor(RobotDrive::kFrontRightMotor, true);
		myRobot->SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
		myRobot->SetInvertedMotor(RobotDrive::kRearRightMotor, true);
		myRobot->SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
		// watchdog can be left alone for 0.1 s before starving
		myRobot -> SetExpiration(0.1);
		//solenoids
		gripOpenSolenoid = new Solenoid(1);
		gripCloseSolenoid = new Solenoid(2);
		// encoders
		
		encoderMainArm = new   Encoder(6, 3, 6, 4, false, Encoder::k4X); 			//CHECK ME ON THIS!!!
		encoderSecondaryArm = new   Encoder(6, 5, 6, 6, false, Encoder::k4X); 			//CHECK ME ON THIS!!!
		encoderMainArm -> Start();
		encoderSecondaryArm -> Start();
		
	//INIT SEQUENCE
	//this zeros out the encoders for the arms
		while(!limitMainTravelPosition -> Get()){
		mainArmController->Set(-0.5);
		}
		encoderMainArm -> Reset();
		
		while(!limitSecondaryTravelPosition -> Get()){
		secondaryArmController->Set(-0.5);
		}
		encoderSecondaryArm -> Reset();
		
	}// end Constructor()

	/**
	 * Drive left & right motors for 2 seconds then stop
	 */
void Autonomous(void) {
		/*GET TUBE
		*/
		double defaultSteeringGain = 0.65;	// default value for steering gain

		int binaryValue;
		int previousValue = 0;
		double steeringGain;

		// the power profiles for the straight and forked robot path. They are
		// different to let the robot drive more slowly as the robot approaches
		// the fork on the forked line case.
		double forkProfile[] = {0.70, 0.70, 0.70, 0.70, 0.60, 0.60, 0.60, 0.60, 0.55, 0.55, 0.50, 0.50, 0.40, 0.40, 0.0};
		double straightProfile[] = {0.70, 0.70, 0.70, 0.70, 0.60, 0.60, 0.60, 0.60, 0.35, 0.35, 0.35, 0.35, 0.35, 0.35, 0.0};

		double *powerProfile;  // the selected power profile
		
		// set the straight vs forked path variables as read from the DS digital
		// inputs or the I/O Setup panel on the driver station.
		bool straightLine = ds->GetDigitalIn(1);
		powerProfile = (straightLine) ? straightProfile : forkProfile;
		double stopTime = (straightLine) ? 2.0 : 4.0;
		bool goLeft = !ds->GetDigitalIn(2) && !straightLine;
		printf("StraightLine: %d\n", straightLine);
		printf("GoingLeft: %d\n", goLeft);

		bool atCross = false;	// true when robot has reached end

		// set up timer for 8 second max driving time and use the timer to
		// pick values from the power profile arrays
		Timer *timer = new Timer();
		timer->Start();
		timer->Reset();
		
		int oldTimeInSeconds = -1;
		double time;
		double speed, turn;

		// loop until either we hit the "T" at the end or 8 seconds has
		// elapsed. The time to the end should be less than 7 seconds
		// for either path.
		while ((time = timer->Get()) < 15.0 && !atCross) {
			int timeInSeconds = (int) time;
			int leftValue = leftLineFollow->Get() ? 1 : 0;	// read the line tracking sensors
			int middleValue = middleLineFollow->Get() ? 1 : 0;
			int rightValue = rightLineFollow->Get() ? 1 : 0;

		    // compute the single value from the 3 sensors. Notice that the bits
		    // for the outside sensors are flipped depending on left or right
		    // fork. Also the sign of the steering direction is different for left/right.
			if (goLeft) {
				binaryValue = leftValue * 4 + middleValue * 2 + rightValue;
				steeringGain = -defaultSteeringGain;
			} else {
				binaryValue = rightValue * 4 + middleValue * 2 + leftValue;
				steeringGain = defaultSteeringGain;
			}

			speed = powerProfile[timeInSeconds];	// speed value for this time
			turn = 0;								// default to no turn

			switch (binaryValue) {
				case 1:					// just the outside sensor - drive straight
					turn = 0;
					break;
				case 7:					// all sensors - maybe at the "T"
				if (time> stopTime) {
					atCross = true;
					speed = 0;
				}
				break;
				case 0:					// no sensors - apply previous correction
				if (previousValue == 0 || previousValue == 1) {
					turn = steeringGain;
				}
				else {
					turn = -steeringGain;
				}
				break;
				default:				// anything else, steer back to the line
				turn = -steeringGain;
			}
			// useful debugging output for tuning your power profile and steering gain
			if(binaryValue != previousValue)
				printf("Time: %2.2f sensor: %d speed: %1.2f turn: %1.2f atCross: %d\n", time, binaryValue, speed, turn, atCross);
			// move the robot forward
			myRobot->ArcadeDrive(speed, turn);
			if (binaryValue != 0) {previousValue = binaryValue;}
			oldTimeInSeconds = timeInSeconds;
			Wait(0.01);
		}
		int placement = 1;			//0 = travel 1 = low   2 = middle  3 = high
		switch (placement){	
			case 0:
				LoadArmPosition();
			break;
			case 1:
				LowArmPosition();
			break;
			case 2:
				MiddleArmPosition();
			break;
			case 3:
				TopArmPosition();
			break;
			default:
			;
		}
				
		GripRelease();
		DefaultArmPosition();
		// stop driving when finished
		myRobot->ArcadeDrive(0.0, 0.0);
	}
	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void)
	{
		myRobot -> SetSafetyEnabled(true);
		while (IsOperatorControl())
		{
			
			// drive with arcade style (use left stick)
			myRobot -> ArcadeDrive(leftJoy); 
			myArm -> ArcadeDrive(rightJoy);  
			Wait(0.005);				// wait for a motor update time
			readJoystick();
		printf("encoderMainArm: %d\n", encoderMainArm -> Get());
		printf("encoderSecondaryArm: %d\n", encoderSecondaryArm -> Get());
		} // end while
	}// end OperatorControl()
	
	
	/**
		read joystick buttons
	*/
	void readJoystick(void)
	{
	lastButton = 0;				//reset the counter
	if(rightJoy -> GetRawButton(low) && rightJoy -> GetRawButton(trigger)){
		if(lastButton != 1){lastButton = 1; LowArmPosition();}
		}
	if(rightJoy -> GetRawButton(middle) && rightJoy -> GetRawButton(trigger)){
		if(lastButton != 2){lastButton = 2; MiddleArmPosition();}
	}
	if(rightJoy -> GetRawButton(high) && rightJoy -> GetRawButton(trigger)){
		if(lastButton != 3){lastButton = 3; TopArmPosition();}
	}
	if(rightJoy -> GetRawButton(load) && rightJoy -> GetRawButton(trigger)){
		if(lastButton != 4){lastButton = 4; LoadArmPosition();}
	}
	if(rightJoy -> GetRawButton(grip) && rightJoy -> GetRawButton(trigger)){
		if(lastButton != 5){lastButton = 5; GripRelease();}
	}
	if(rightJoy -> GetRawButton(defaultArm) && rightJoy -> GetRawButton(trigger)){
		if(lastButton != 6){lastButton = 6; DefaultArmPosition();}
	}
	if(rightJoy -> GetRawButton(10)){gripOpenSolenoid -> Set(true);}
	if(!rightJoy -> GetRawButton(10)){gripOpenSolenoid -> Set(false);}
	if(rightJoy -> GetRawButton(11)){gripCloseSolenoid -> Set(true);}
	if(!rightJoy -> GetRawButton(11)){gripCloseSolenoid -> Set(false);}
	
	}//end readJoystick
	
	void LowArmPosition(void)
	{
		if(isDefaultArmPosition){DefaultArmPosition();}
	isDefaultArmPosition = false;
	printf("Entering LowArmPosition");
	MoveArm(100, 100);
	}
	
	void MiddleArmPosition(void)
	{
		if(isDefaultArmPosition){DefaultArmPosition();}
		isDefaultArmPosition = false;
		printf("Entering MiddleArmPosition");
	MoveArm(100, 100);
	}// end MiddleArmPosition()
	
	void TopArmPosition(void)
	{
		if(isDefaultArmPosition){DefaultArmPosition();}
		isDefaultArmPosition = false;
		printf("Entering TopArmPosition");
	MoveArm(100, 100);
	}// end TopArmPosition()
		
	void LoadArmPosition(void)
	{
		if(isDefaultArmPosition){DefaultArmPosition();}
		isDefaultArmPosition = false;
		printf("Entering LoadArmPosition");
	MoveArm(100, 100);
	}// end LoadArmPosition()
	
	void DefaultArmPosition(void)
	{
		isDefaultArmPosition = true;
		printf("Entering TravelPosition");
	
	while(encoderSecondaryArm -> Get() > 0)
		{
		secondaryArmController->Set(-0.5);			//speed for moving the arm
		}
		
	while(encoderMainArm -> Get() > 0)
		{
		mainArmController->Set(-0.5);			//speed for moving the arm
		}
		
	}// end DefaultArmPosition()
	
	/*
	the function to move the arm
	main = main arm
	secondary = secondary arm
	*/
	void MoveArm(int Main, int Secondary)
	{
		//mainArm
		desiredAngle = Main;							//manually set this variable per arm angle
		previousAngle = encoderMainArm -> Get();
		while(previousAngle < desiredAngle)
		{
		mainArmController->Set(0.5);			//speed for moving the arm
		}
		
		//secondaryArm
		desiredAngle = Secondary;							//manually set this variable per arm angle
		previousAngle = encoderSecondaryArm -> Get();
		while(previousAngle < desiredAngle)
		{
		secondaryArmController->Set(0.5);			//may have to change positive/negative value //speed for moving the arm
		}
		
	}// end LowArmPosition()
	
	void GripRelease(void)
	{
		if(gripState){	//if open close the gripper
			gripState = false;
			gripCloseSolenoid->Set(true);
			Wait(1.0);
			gripCloseSolenoid->Set(false);
		} else {// open the gripper
			gripState = true;
			gripOpenSolenoid->Set(true);
			Wait(1.0);
			gripOpenSolenoid->Set(false);
		}
	}// end GripRelease()
	
};
START_ROBOT_CLASS(MurrayCallowayRobot2011);
